#include <debug.h>
#include <string.h>

/* Variables. */
 /* Save the X position. */
 static int xpos;
 /* Save the Y position. */
 static int ypos;
 /* Point to the video memory. */
 static volatile unsigned char *video;
 
/**
 * Halt the current thread
 */
void pause(void)
{
	for (;;){ asm("hlt"); }
}

/**
 * Print goodies
 * 
 * TODO: fix
 */
void debugf(const char *format, ...)
{
	char **arg = (char **) &format;
	int c;
	char buf[20];
	int padchar;
	int width;
 
	arg++;
	while ((c = *format++) != 0)
    {
    	if (c != '%')
        	putchar (c);
       	else
        {
        	
			padchar = ' ';
			width = 0;
			c = *format++;
			if( c == '0' ){
				padchar = '0';
				c = *format++;
			}
			while( c >= '0' && c <= '9' ){
				width *= 10;
				width += c - '0';
				c = *format++;
			}
			
        	char *p;
			//c = *format++;
           	switch (c)
            {
            	case 'd':
 				case 'u':
 				case 'x':
 					itoa(buf, c, *((int *) arg++));
               		p = buf;
               		goto string;
               		break;
             	case 's':
   					p = *arg++;
   					if (! p)
     					p = "(null)";
     					
     				if (width > 0)
     				{
     					while (width-- > 0)
     						putchar(*p++);
     					break;
     				}
 
             	string:
             		width = width - strlen(p);
             		while (width-- > 0)
             			putchar(padchar);
               		while (*p)
                 		putchar (*p++);
           		break;
 
             	default:
               		putchar (*((int *) arg++));
               	break;
             }
         }
     }
}

// TODO: comment and clean
void itoa (char *buf, int base, int d)
{
	char *p = buf;
   	char *p1, *p2;
   	unsigned long ud = d;
   	int divisor = 10;
 
   	/* If %d is specified and D is minus, put `-' in the head. */
   	if (base == 'd' && d < 0)
 	{
   		*p++ = '-';
       	buf++;
       	ud = -d;
    }
   	else if (base == 'x')
    	divisor = 16;
 
   	/* Divide UD by DIVISOR until UD == 0. */
   	do
    {
    	int remainder = ud % divisor;
 
       	*p++ = (remainder < 10) ? remainder + '0' : remainder + 'A' - 10;
    }
   	while (ud /= divisor);
 
   	/* Terminate BUF. */
   	*p = 0;
 
   	/* Reverse BUF. */
   	p1 = buf;
   	p2 = p - 1;
   	while (p1 < p2)
    {
    	char tmp = *p1;
       	*p1 = *p2;
       	*p2 = tmp;
       	p1++;
       	p2--;
   	}
}
 
 /* Put the character C on the screen. */
void putchar (int c)
{
	if (c == '\n' || c == '\r')
    {
    	newline:
       	xpos = 0;
       	ypos++;
       	if (ypos >= LINES)
       	{
       		ypos = LINES - 1;
       		//scroll
       		int b;
       		for (b = 0; b < ypos*COLUMNS*2; b++)
       		{
       			*(video + b) = *(video + (b + COLUMNS * 2));
       		}
       		
       		for (b = 0; b < COLUMNS*2; b++)
       		{
       			*(video + b + ypos *COLUMNS * 2) = 0;
       		}
       	}
       	return;
    }
 
   	*(video + (xpos + ypos * COLUMNS) * 2) = c & 0xFF;
   	*(video + (xpos + ypos * COLUMNS) * 2 + 1) = ATTRIBUTE;
 
   	xpos++;
   	if (xpos >= COLUMNS)
    	goto newline;
}

void putchar_at(int c, int attrib, int x, int y)
{
	*(video + (x + y * COLUMNS) * 2) = c & 0xFF;
	*(video + (x + y * COLUMNS) * 2 + 1) = attrib;
}

 /* Clear the screen and initialize VIDEO, XPOS and YPOS. */
void cls (void)
{
   	int i;
 
   	video = (unsigned char *) VIDEO;
 
   	for (i = 0; i < COLUMNS * LINES * 2; i++)
     	*(video + i) = 0;
 
   	xpos = 0;
   	ypos = 0;
 }
